#coding=utf-8
import time

import numpy as np
'''
    3D自由落体模型（含有风阻）
'''

class FreeDropBinder:
    '''
        为实体绑定自由落体属性
    '''
    def __init__(self,windSpd=np.array([0.0,0.0,0.0]),
                 resRatio=0.0004,
                 G=9.8,
                 initialPos=np.array([0.0,0.0,0.0]),
                 initialSpd=np.array([0.0,0.0,0.0]),
                 randRatio=0.1):
        '''
        :param windSpd:  风速（三维）
        :param resRatio: 风阻比例（全向）
        :param G: 重力加速度
        :param initialPos:  物体初始位置
        :param initialSpd:  物体初始速度
        '''
        self.position=initialPos
        self.spd=initialSpd
        self.windSpd=windSpd
        self.resRatio=resRatio
        self.G=G
        self.randRatio=randRatio

    def StateUpdate(self,delta_t=0.05,driveForce=np.array([0.0,0.0,0.0])):
        '''
        更新实体位置信息
        :param delta_t:
        :return:
        '''
        #   重力因素
        self.spd+=np.array([0,0,-self.G*delta_t])
        #   风阻因素
        self.spd=np.where(self.spd>0,self.spd-self.resRatio*self.spd*self.spd,self.spd)
        self.spd = np.where(self.spd <= 0, self.spd + self.resRatio * self.spd * self.spd, self.spd)
        #   风力因素

        #   驱动因素
        self.spd+=(driveForce+self.windSpd)*delta_t
        #   随机因素
        self.spd+=(np.random.rand(3)-0.5)*2*self.randRatio*delta_t
        #   更新坐标
        self.position=self.position+self.spd*delta_t

if __name__=='__main__':

    box=FreeDropBinder(initialSpd=np.array([10.0,0.0,100.0]))

    for i in range(30):
        print(box.Update())




